import numpy as np
import Common.MatrixProcess as mp
from Common.ControlEnum import forcemode
from scipy.spatial.transform import Rotation as R

class AddmittanceControl():
    def __init__(self,k3,b3,m3,f3,dt,trans=True,teachmode=forcemode.FreeMode):
        self.k=k3
        self.b=b3
        self.m=m3
        self.dt=dt*120
        self.trans=trans
        self.teachmode=teachmode
        print(f'导纳算法刚度矩阵k={self.k},b={self.b},m={self.m},dt={self.dt},是否转换力坐标：{trans}')

        self.goalFt = f3#工具坐标系下的f3
        self.times = 6170
        self.i = 0
        self.delta_pose_t0=np.zeros((3))
        self.delta_vel_t0 = np.zeros((3))
        self.delta_acc_t0 = np.zeros((3))

        self.delta_pose_t1=np.zeros((3))
        self.delta_vel_t1 = np.zeros((3))
        self.delta_acc_t1 = np.zeros((3))
        self.goalpose_old = np.zeros((6))
        self.goalpose_v = np.zeros((3))

        #lisen 添加 start
        self.goalpose_v_old = np.zeros((3))
        self.goalpose_a = np.zeros((3))

        self.i

        #region 力模式初始化
        if (teachmode == forcemode.TeachMode):
            self.saveforce = []#示教力
        elif (teachmode==forcemode.ReplayMode):
            self.readforce=[]#复现示教的力
            self.readforce=np.loadtxt("demoforce.txt")
        else:
            None
        #endregion

    def get_save_value(self):
        return self.saveforce

    def TorqueTrans(self,istrans,pose,force):
        """
        是否需要将力转化至基座标系下
        Args:
            istrans:
            pose:
            force:

        Returns:

        """
        if(istrans==True):
            RBase_Tcp = mp.rotation_matrix_from_vectorr(pose[3:])#旋转向量转旋转矩阵
            torque_base_cur = RBase_Tcp.dot(force[3:])#转化力矩
            return torque_base_cur
        else:
            return force[3:]

    def forceCal(self,teachmode,curforce):
        """
        根据模式，选择是复现力，还是记录力
        Args:
            teachmode:
            force:当前的力

        Returns:

        """
        if(teachmode==forcemode.TeachMode):
            #Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
            Fe = curforce[:3] - self.goalFt

            self.saveforce.append(curforce[:3])
        if(teachmode==forcemode.FreeMode):
            Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
        if(teachmode==forcemode.ReplayMode):
            curforce[:2] = 0.1*curforce[:2] + self.readforce[self.i][:2]
            Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b - (self.m - self.b * self.dt) * self.goalpose_a[:3]
        return Fe

    #力是映射到单个角度的增量，
    #再有角度的增量计算出旋转矢量，RX*RY*RZ，再左乘完成导纳控制
    def calpose(self,curpose,goalpose,curforce):
        """
        Args:
            curpose:当前位置，从机器人读取上来的
            goalpose: 目标轨迹，世界坐标系下的
            curvel:
            curforce:

        Returns:
        """

        ###1
        torque=self.TorqueTrans(self.trans,curpose,curforce)
        print(torque)
        ###2
        if(self.i ==0):
            delta=self.__cal_eular_delta_vec(goalpose[3:],curpose[3:])#将旋转向量换算成角度
            self.goalpose_v=delta/self.dt  #给速度初始化,只执行一次
            self.goalpose_a = 2*(goalpose[3: ]-curpose[3: ])/self.dt/self.dt# 加速度初始化 用x=1/2*a*t^2计算

            #lisen 添加 end
        else:
            delta=self.__cal_eular_delta_vec(goalpose[3:],self.goalpose_old[3:])#将旋转向量换算成角度
            self.goalpose_v= delta/self.dt
            self.goalpose_a = (self.goalpose_v - self.goalpose_v_old) / self.dt
        delta = self.__cal_eular_delta_vec(goalpose[3:], curpose[3:])  # 将旋转向量换算成角度

        self.goalpose_old = goalpose.copy()#获取当前的目标姿态
        self.goalpose_v_old = self.goalpose_v.copy()#获取当前的目标速度

        ###3
        torque[np.abs(torque)<0.2]=0
        # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-self.m * (2*self.goalpose_a[:3] - 0.002)
        Te=self.forceCal(self.teachmode,torque)

        ###4
        Ve=self.delta_vel_t0
        Xe=self.delta_pose_t0
        self.delta_acc_t1 = (1 / self.m) * (Te - self.b * Ve)
        self.delta_vel_t1= Ve + self.delta_acc_t1 * self.dt
        ###4.1 限速
        sump = np.linalg.norm(self.delta_vel_t1)
        if (sump > 0.7):
            self.delta_vel_t1 = (0.7 / sump) * self.delta_vel_t1
        self.delta_pose_t1= Xe + self.delta_vel_t1 * self.dt#x0+vt
        self.delta_pose_t0=self.delta_pose_t1
        self.delta_vel_t0=self.delta_vel_t1

        print(f"当前力大小，{Te},调整角度：{self.delta_pose_t1}")
        print(f"当前力大小，{Te},调整角度：{self.delta_pose_t1}")
        movepose=self.__calcmdpose(goalpose,self.delta_pose_t1)#
        self.i = self.i + 1
        return movepose#这个是最终的坐标位置，基坐标系下的

    def __calcmdpose(self,goalpose,deltapose):
        """

        Args:
            goalpose:目标位姿
            deltapose: 旋转方向的调整量

        Returns:调整后下发给机器人的位姿

        """
        movepose = np.zeros(6)
        movepose[:3]=goalpose[:3]  # 最终是加在goalpose上面了
        #待定义，delatpose的形式是什么？是旋转矢量吗，还是角度增量。
        #答：是在XYZ三个方向上角度的增量，力直接引起的是绕XYZ轴三个方向的旋转
        pose_vec=self.__cal(goalpose,deltapose[0],deltapose[1],deltapose[2])
        # print("delta_pose_t1=",self.delta_pose_t1)
        movepose[3:] = pose_vec
        return movepose

    def __cal(self, curpose,x, y, z):
        """
        传入xyz方向的增量，以及当前的姿态，计算目标姿态
        Args:
            curpose:当前的位姿
            x:姿态X方向的增量
            y:姿态Y方向的增量
            z:姿态Z方向的增量
        Returns:

        """
        cur_pose = curpose
        cur_roate = cur_pose[3:]
        #print(f'当前旋转矢量{cur_roate}')

        # 将旋转角度转换为旋转对象（单位为弧度，需用np.radians转换）
        rotation_x = R.from_euler('x', np.radians(x), degrees=False)
        rotation_y = R.from_euler('y', np.radians(y), degrees=False)
        rotation_z = R.from_euler('z', np.radians(z), degrees=False)
        target_rotation = rotation_z * rotation_y * rotation_x  # 组合旋转：Z -> Y -> X
        #cal_eular(target_rotation)  # 验证一下结果
        # target_rotation = rotation_x * rotation_y * rotation_z
        current_rotation = R.from_rotvec(cur_roate)  # 计算当前的旋转矩阵
        # new_rotation = current_rotation * target_rotation#右乘
        new_rotation = target_rotation * current_rotation  # 左乘得到新的坐标
        new_rotation_vector = new_rotation.as_rotvec()  # 新姿态转化为旋转矢量，并返回

        ###打印验证可不用
        # cal_eular_delta(new_rotation, current_rotation.inv())
        # cur_euler = current_rotation.as_euler('xyz', degrees=True)
        # euler_angles = new_rotation.as_euler('xyz', degrees=True)
        # print(f'cureuler:{cur_euler},euler:{euler_angles}')
        ###
        return new_rotation_vector

    def __cal_eular_delta_vec(self,V1, V2):
        """
        delta = V1*V2.inv()#左乘
        Args:
            V1: 目标旋转向量
            V2: 当前旋转向量

        Returns:

        """
        R1 = R.from_rotvec(V1)
        R2 = R.from_rotvec(V2)
        eular = self.__cal_eular_delta(R1, R2.inv())
        #print(eular)
        return eular

    def __cal_eular_delta(self,R1, R2):
        R0 = R1 * R2
        R0 = R0.as_euler('xyz', degrees=True)
        R0 = np.round(R0, 2)
        #print(f'欧拉角差值{R0}')
        return R0